#include "pid.h"
#include "filt.h"
#include "avoidance.h"

//PID里的全局变量
u8 basicV;
#define ultrthreslold 6 //fix:参数还没调
//ultr相关的别名
sbit Trig_Pin = P3^6; //发送
sbit Echo_Pin = P3^3; //接受

void setPWMTimer(void)
{
	//初始化PWM
	TH0=0XFC; //PWM定时器，定时1ms
	TL0=0X18;	
	ET0=1; //打开定时器0中断允许
	EA=1; //打开总中断
	TR0=1; //打开定时器
}

void initInterrput(void)
{
	//开外部中断
	IT0=1; //跳变沿出发方式（下降沿）
	EX0=1; //打开INT0的中断允许。		
	//开计时器中断
	TMOD|=0X11;//两个计时器都是方式1
	setPWMTimer();
}

void MeterByTrig()
{
	unsigned char tempa;
	unsigned char IsOverFlow;
	unsigned long PresentTime, tempb;
	IsOverFlow = 0;

	initInterrput();//按例子里的再初始化一遍

	//发射
	Trig_Pin = 0;
	for(tempa = 0; tempa < 10; tempa++)
	{
		Trig_Pin = 1;
	}
	Trig_Pin = 0;

	while(1)
	{
		//发射结束，并接收到信号之后，Echo_Pin==1持续一段时间，时长为测距结果，检测到为1后使用计时器计算持续的时长
		if(Echo_Pin == 1)
		{
			//start timer
			TCON = 0X40;
			break;
		}
		if(TCON&0X80)
		{
			IsOverFlow = 1;
			break;//overflow;
		}
	}

	while(1)
	{
		if(Echo_Pin == 0)
		{
			//Stop Timer
			TCON = 0X00;
			break;
		}
		if(TCON&0X80)
		{
			IsOverFlow = 1;
			break;//overflow;
		}
	}
	
	setPWMTimer(); //先前破坏了PWM计时器，需要重设

	//unsigned long PresentTime, tempb;
	PresentTime = TH1;
	PresentTime <<= 0X08;
	PresentTime += TL1;

	tempb = PresentTime/137;
	PresentTime /= 7;//*3

	PresentTime -= tempb;//time, us

	tempb = PresentTime/150;
	PresentTime /= 3;
	PresentTime += tempb;
	
	if(PresentTime>ultrthreslold)
		avoidance();
}

void main(void)
{
	filtInit(); //初始化超声波用的滤波记录
	initInterrput();	
	setBasicV(9); //fix:暂且先这么设，平道加速这个还需要动态调整
	while(1)
	{
		MeterByTrig();
		if(isCrossing()!=None)
		{
			//处理交叉点
		}
		else
		{
			if(!isOnline())
				resetV();
			else
				recoverV();
		}
	}
}